/*
 * Avoibot32.c
 *
 * Created: 13-03-2013 23:55:16
 *  Author: Karmu
 */ 

/*
TIMER Information
-----------------
TIMER0 - used for PID interrupts
TIMER1 - used for PWM for motor drives
TIMER2 - used for SERVO
*/

/* define CPU frequency in Mhz here if not defined in Makefile */
#ifndef F_CPU
#define F_CPU 11059200UL			/* 11.0592 MHz crystal clock */ //low fuse EF
//#define F_CPU 1000000UL			/* 1 MHz internal RC clock */
//#define F_CPU 8000000UL			/* 8 MHz internal RC clock */ //low fuse E4
#endif

#include <avr/io.h>
#include <avr/interrupt.h>   
#include <util/delay.h>
#include <util/atomic.h>
#include <avr/pgmspace.h>
#include <stdlib.h>
#include <string.h>
#include <assert.h>

#include "lcd.h"  // Using Peter Fleury's LCD library
//#include "AVR035.h"
#include "motor.h"
#include "ps2mouse.H"
#include "servo.h"
#include "uartcmd.h" // Using Peter Fleury's UART library

/*UART baudrate */
#define UART_BAUD_RATE      115200

#define DELAY		1000

/* defines for LCD (other defines are in LCD.h) */
#define LCDDELAY	50

/* define PWM for Motor Speed control */
/* define TOP & BOTTOM for PWM (ICR1) */
#define TOP			10000 //8 MHz 1 Pre-scalar
#define BOTTOM_R		4300
#define BOTTOM_L		4150

/* Motor PS2 ENCODER has 36 slots thus, one wheel revolution is 72 clicks on quad encoder on PS2 protocol*/
#define REVOLUTION 72

/* constant definitions for LCD */
static const PROGMEM unsigned char copyRightChar[] =
{
	0x07, 0x08, 0x13, 0x14, 0x14, 0x13, 0x08, 0x07,
	0x00, 0x10, 0x08, 0x08, 0x08, 0x08, 0x10, 0x00
};
static const PROGMEM unsigned char blockChar[] =
{
	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF
};

/*
 PID control variables 
*/
int prevErrorLeft; //left motor previous error
int prevErrorRight; //right motor previous error
int IerrorLeft=0; //left motor integral error
int IerrorRight=0; //right motor integral error
int encoderCountLeft = 0;
int encoderCountRight = 0;
int setpoint = 30; //setpoint
uint8_t runPIDcount=0; // run PID loop only 10 times a second
volatile uint16_t secondsCount=0; // variable to count seconds in timer0 ISR
volatile int16_t actualPointLeft=0,actualPointRight=0;
volatile uint8_t SEND_ON_UART=0;
/*.....................................................*/
//Left PID constants
int KPl = 15; //PID proportional gain constant
float KIl = 0.1; //PID integral gain constant
float KDl = 220; //PID derivative gain constant
//Right PID constants
int KPr = 15; //PID proportional gain constant
float KIr = 0.4; //PID integral gain constant
float KDr = 200; //PID derivative gain constant
/*.....................................................*/
/* Temp values */
uint8_t startstop = 0;
uint8_t selectcourse = 1;
uint8_t option=0;
uint8_t state;
uint8_t OK;
volatile uint8_t UPDATE_UART=0;
char buffer[7];	// buffer used for itoa conversion in timer0 ISR
char strbuffer[40];

#define BUTTONDEBOUCE 200
#define MENU		51
#define SETTINGS	52
#define RUN			53
#define COURSE		54
#define PIDL		55
#define PIDR		56

/* function prototypes */
void init();
void timer0_Init(void);
void booting();
void speed_control(int);
void doPID(void);
void doPIDRight(void);
void doPIDLeft(void);
/*uint8_t getUARTData(volatile char *buf, uint8_t bytesToRead) {

	uint8_t i = 0;
	while (bytesToRead) {
		unsigned int datum = uart_getc();
		
		if (datum & 0x0F00) {
			break;
		}
		buf[i] = (uint8_t)datum;
		bytesToRead--;
		i++;
	}

	// Terminate with a null byte.
	buf[i] = '\0';

	return i;
}
char** str_split(char* a_str, const char a_delim)
{
    char** result    = 0;
    size_t count     = 0;
    char* tmp        = a_str;
    char* last_comma = 0;

    // Count how many elements will be extracted. 
    while (*tmp)
    {
        if (a_delim == *tmp)
        {
            count++;
            last_comma = tmp;
        }
        tmp++;
    }

    // Add space for trailing token. 
    count += last_comma < (a_str + strlen(a_str) - 1);

    // Add space for terminating null string so caller knows where the list of returned strings ends. 
    count++;

    result = malloc(sizeof(char*) * count);

    if (result)
    {
        size_t idx  = 0;
        char* token = strtok(a_str, ",");

        while (token)
        {
            assert(idx < count);
            *(result + idx++) = strdup(token);
            token = strtok(0, ",");
        }
        assert(idx == count - 1);
        *(result + idx) = 0;
    }

    return result;
}

*/
int mod (int a, int b)
{
	if(b < 0) //you can check for b == 0 separately and do what you want
	return mod(-a, -b);
	int ret = a % b;
	if(ret < 0)
	ret+=b;
	return ret;
}
int main(void)
{
	/* Declarations */
	int16_t count_left;
	int16_t count_right;
	int8_t lastcourse;
	const char* course[]={"NOT DEFINED     ",
						  "GO_FORWARD      ",
						  "GO_REVERSE      ",
						  "GO_LEFT         ",
						  "GO_RIGHT        ",
						  "GO_HARD_LEFT    ",
						  "GO_HARD_RIGHT   ",
						  "GO_BRAKE        "};
	//int8_t sav;
	
	/* initialize MOTOR and PWM */
	motor_init();
	motor_pwm_timer1_init(TOP,BOTTOM_R,BOTTOM_L);

	/* Initialize SERVO*/
	servo_init(8,26);
	servo_set_position(90);
	
	/* mouse initialize */
	mouseInit();
	
	/* initialize LCD display, cursor off */
	lcd_init(LCD_DISP_ON);
	lcd_clrscr();   /* clear display home cursor */

	/* Booting animation */
	booting();
	lcd_clrscr();
	
	/* Initialize timer0 and enable overflow interrupt to read Encoders */
	timer0_Init();

    /*
     *  Initialize UART library, pass baudrate and AVR cpu clock
     *  with the macro 
     *  UART_BAUD_SELECT() (normal speed mode )
     *  or 
     *  UART_BAUD_SELECT_DOUBLE_SPEED() ( double speed mode)
     */
    uart_init( UART_BAUD_SELECT(UART_BAUD_RATE,F_CPU) );

	/* initializations */
	init();
	
	selectcourse=GO_BRAKE;
	uart_puts("Start the Bot...\r\n");
	for(int i=0;i<=7;i++){
		uart_puti(i);
		uart_puts("\t");
		uart_puts(course[i]);
		uart_puts("\r\n");
	}
	while(1){
		if (SEND_ON_UART==1) {
			if (UPDATE_UART==1) {
				itoa( secondsCount, buffer, 10);		// convert integer into string (decimal format)
				strcpy(strbuffer,buffer);

				strcat(strbuffer,",");
				itoa( actualPointLeft*4, buffer, 10);		// convert integer into string (decimal format)
				strcat(strbuffer,buffer);

				strcat(strbuffer,",");
				itoa( actualPointRight*4, buffer, 10);	// convert integer into string (decimal format)
				strcat(strbuffer,buffer);

				strcat(strbuffer,"\r\n");

				uart_puts(strbuffer);					// and transmit string to UART

				UPDATE_UART=0;
			}
			SEND_ON_UART=0;
		}
		process_uart();
		if(show==0){ // show when value is 0
			uart_puts("-----\r\n");
			uart_puts("Avoibot PID configuration\r\n");
			uart_puts("KPL\tKIL\tKDL\tKPR\tKIR\tKDR\tRUN\r\n");
			uart_puti(KPl);uart_puts("\t");
			uart_puti(KIl);uart_puts("\t");
			uart_puti(KDl);uart_puts("\t");
			uart_puti(KPr);uart_puts("\t");
			uart_puti(KIr);uart_puts("\t");
			uart_puti(KDr);uart_puts("\t");
			uart_puts(course[run]);
			uart_puts("\r\n");
			show=1;
		}		
		KPl=kpl;
		KIl=kil;
		KDl=kdl;
		KPr=kpr;
		KIr=kir;
		KDr=kdr;
		selectcourse=run;
		if(start==0 && stop!=0 && selectcourse>0 && selectcourse<8){
			count_left=mouseY;
			count_right=mouseX;
			
			lcd_putsxy("L",9,0);
			lcd_putsxy("R",0,0);
			
			lcd_putixy(count_left,9,1,6);
			lcd_putixy(count_right,0,1,6);

			lcd_putixy(actualPointLeft,10,0,5); //left
			lcd_putixy(actualPointRight,01,0,5); //right
			
			// Start the Robot
			steer_robot(selectcourse);
			UPDATE_UART=1;
		}			
		else if (stop==0 && start==0){
			steer_robot(GO_BRAKE);
			lcd_clrscr();
			lcd_putsxy("STOPPED",0,0);
			lcd_putixy(OCR1B,9,0,5);
			lcd_putsxy("R",15,0);
			lcd_putixy(OCR1A,9,1,5);
			lcd_putsxy("L",15,1);
			start=1;
			stop=1;
			steer_robot(GO_BRAKE);
			UPDATE_UART=0;
		}
		if (isMiddle) {
			_delay_ms(BUTTONDEBOUCE*2);
			if (start==0){
				stop=0;
			}
			if (start==1){
				start=0;
			}
		}

	}

	/* Main loop 
	steer_robot(GO_BRAKE);
	state=MENU;
	
	while(1){
		switch(state){
			OK=0;
			case MENU:
				mouseZ=1;
				lcd_clrscr();
				lcd_putsxy("MAIN MENU:      ",0,0);
				while(!OK){
					option=mod(mouseZ,2);
					if (option==0) option=2;
					switch(option){
						case 1:
							lcd_putsxy("Run    ",2,1);
							lcd_putixy(OCR1B,9,0,5);
							lcd_putsxy("R",15,0);
							lcd_putixy(OCR1A,9,1,5);
							lcd_putsxy("L",15,1);
							state=RUN;
							break;
						case 2:
							lcd_putsxy("MAIN MENU:      ",0,0);
							lcd_putsxy("Settings      ",2,1);
							state=SETTINGS;
							break;
					}
					lcd_putixy(option,0,1,1);
					if (isMiddle) {
						_delay_ms(BUTTONDEBOUCE*2);
						OK=1;
						lcd_clrscr();
					}
				}
				OK=0;			
				break;
			case SETTINGS:
				mouseZ=1;
				lcd_clrscr();
				lcd_putsxy("SETTINGS MENU:  ",0,0);
				while(!OK){
					option=mod(mouseZ,4);					
					if (option==0) option=4;
					lcd_putixy(option,0,1,1);
					switch(option){
						case 1:lcd_putsxy("Course     ",2,1);
							state=COURSE;
							break;
						case 2:lcd_putsxy("PID Right  ",2,1);
							state=PIDR;
							break;
						case 3:lcd_putsxy("PID Left   ",2,1);
							state=PIDL;
							break;
						case 4:lcd_putsxy("Back       ",2,1);
							state=MENU;
							break;
					}
					
					if (isMiddle) {
						_delay_ms(BUTTONDEBOUCE*2);
						OK=1;
					}
				}
				OK=0;
				break;
			case COURSE:
				mouseZ=1;
				lcd_clrscr();
				lcd_putsxy("COURSE MENU:    ",0,0);
				while(!OK){
					selectcourse=mod(mouseZ,7);
					if (selectcourse==0) selectcourse=7;
					switch(selectcourse){
						case 1:lcd_putsxy("Forward    ",2,1);
							break;
						case 2:lcd_putsxy("Reverse    ",2,1);
							break;
						case 3:lcd_putsxy("Left       ",2,1);
							break;
						case 4:lcd_putsxy("Right      ",2,1);
							break;
						case 5:lcd_putsxy("Hard Left  ",2,1);
							break;
						case 6:lcd_putsxy("Hard Right ",2,1);
							break;
						case 7:lcd_putsxy("Break      ",2,1);
							break;
					}						
					lcd_putixy(selectcourse,0,1,1);
					if (isMiddle) {
						_delay_ms(BUTTONDEBOUCE*2);
						OK=1;
						state=SETTINGS;
					}
				}
				OK=0;
				break;
			case PIDL:
				  // 0123456789012345
				  // ----------------
				  // *KPl*KIl*KDl*Sav
				  //   0   0   0   1 
				lcd_clrscr();
				sav=1;
				OK=0;
				lcd_clrscr();
				lcd_putsxy(" KPl KIl KDl Sav",0,0);
					lcd_putixy(KPl, 0,1,4);
					lcd_putixy(KIl, 4,1,4);
					lcd_putixy(KDl, 8,1,4);
					lcd_putixy(sav,12,1,4);
				while(!OK){
							lcd_putsxy("*", 0,0);
							mouseZ=KPl;
							while(!OK){
								lcd_putixy(KPl, 0,1,4);
								KPl=mouseZ;
								lcd_putixy(KPl, 0,1,4);
								if (isMiddle) {
									_delay_ms(BUTTONDEBOUCE*2);
									OK=1;
								}
							}
							OK=0;

							lcd_putsxy(" ", 0,0);
							lcd_putsxy("*", 4,0);
							mouseZ=KIl;
							while(!OK){
								lcd_putixy(KIl, 4,1,4);
								KIl=mouseZ;
								lcd_putixy(KIl, 4,1,4);
								if (isMiddle) {
									_delay_ms(BUTTONDEBOUCE*2);
									OK=1;
								}
							}
							OK=0;

							lcd_putsxy(" ", 4,0);
							lcd_putsxy("*", 8,0);
							mouseZ=KDl;
							while(!OK){
								KDl=mouseZ;
								lcd_putixy(KDl, 8,1,4);
								if (isMiddle) {
									_delay_ms(BUTTONDEBOUCE*2);
									OK=1;
								}
							}
							OK=0;

							lcd_putsxy(" ", 8,0);
							lcd_putsxy("*",12,0);
							mouseZ=sav;
							while(!OK){
								sav=mod(mouseZ,2);
								lcd_putixy(sav, 12,1,4);
								if (isMiddle) {
									_delay_ms(BUTTONDEBOUCE*2);
									OK=1;
								}
							}	
							if (sav==0) sav=1;
							else {
								OK=1;
								state=SETTINGS;
							}
				}					
				OK=0;
				break;
			case PIDR:
				  // 0123456789012345
				  // ----------------
				  // *KPl*KIl*KDl*Sav
				  //   0   0   0   1 
				lcd_clrscr();
				sav=1;
				OK=0;
				lcd_clrscr();
				lcd_putsxy(" KPr KIr KDr Sav",0,0);
					lcd_putixy(KPr, 0,1,4);
					lcd_putixy(KIr, 4,1,4);
					lcd_putixy(KDr, 8,1,4);
					lcd_putixy(sav,12,1,4);
				while(!OK){
							lcd_putsxy("*", 0,0);
							mouseZ=KPr;
							while(!OK){
								lcd_putixy(KPr, 0,1,4);
								KPr=mouseZ;
								lcd_putixy(KPr, 0,1,4);
								if (isMiddle) {
									_delay_ms(BUTTONDEBOUCE*2);
									OK=1;
								}
							}
							OK=0;

							lcd_putsxy(" ", 0,0);
							lcd_putsxy("*", 4,0);
							mouseZ=KIr;
							while(!OK){
								lcd_putixy(KIr, 4,1,4);
								KIr=mouseZ;
								lcd_putixy(KIr, 4,1,4);
								if (isMiddle) {
									_delay_ms(BUTTONDEBOUCE*2);
									OK=1;
								}
							}
							OK=0;

							lcd_putsxy(" ", 4,0);
							lcd_putsxy("*", 8,0);
							mouseZ=KDr;
							while(!OK){
								KDr=mouseZ;
								lcd_putixy(KDr, 8,1,4);
								if (isMiddle) {
									_delay_ms(BUTTONDEBOUCE*2);
									OK=1;
								}
							}
							OK=0;

							lcd_putsxy(" ", 8,0);
							lcd_putsxy("*",12,0);
							mouseZ=sav;
							while(!OK){
								sav=mod(mouseZ,2);
								lcd_putixy(sav, 12,1,4);
								if (isMiddle) {
									_delay_ms(BUTTONDEBOUCE*2);
									OK=1;
								}
							}	
							if (sav==0) sav=1;
							else {
								OK=1;
								state=SETTINGS;
							}
				}					
				OK=0;
				break;
			case RUN:
								
				count_left=mouseY;
				count_right=mouseX;
				
				lcd_putsxy("L",9,0);
				lcd_putsxy("R",0,0);
				
				lcd_putixy(count_left,9,1,6);
				lcd_putixy(count_right,0,1,6);

				lcd_putixy(actualPointLeft,10,0,5); //left
				lcd_putixy(actualPointRight,01,0,5); //right
				
				// Start the Robot Forward
				steer_robot(selectcourse);
				if (isMiddle) {
					_delay_ms(BUTTONDEBOUCE*2);
					state=MENU;
					steer_robot(GO_BRAKE);
				}
				UPDATE_UART=1;
				break;
		}
	}
	*/
}

void speed_control(int newsetpoint){

	setpoint = newsetpoint;
}



void init()
{
		/* set INT0 and INT1 interrupt enable bit */
		//GICR |= _BV(INT0) | _BV(INT1);
		
		/* The any logical change on INT0 and INT1 generates an interrupt request. */
		//MCUCR |= _BV(ISC10) | _BV(ISC00);
		
		/* enable global interrupts */
		sei();
}

void booting()
{
	int i;
	lcd_puts("-- Booting... --\n");
	/*
    * load two user defined characters from program memory
    * into LCD controller CG RAM location 0 and 1
    */
	lcd_command(_BV(LCD_CGRAM));  /* set CG RAM start address 0 */
	for(i=0; i<16; i++)
	{
		lcd_data(pgm_read_byte_near(&blockChar[i]));
	}
	for(i=0; i<16; i++)
    {
		/* create illusion of progress bar */
		_delay_ms(LCDDELAY);
		/* increment cursor to position i on line 2 */
        lcd_gotoxy(i,1);  
        /* write single char to display */
        lcd_putc(0);
	}	
	
	lcd_clrscr();
	lcd_puts("Avoibot Running\n");
}

void timer0_Init(void)
{
	TCNT0=0x00;
	TCCR0 |= (1 << WGM01 ); // Configure timer 0 for CTC mode
	//TIMSK|=(1<<TOIE0); //Enable timer0 Overflow interrupts
	TIMSK |= (1 << OCIE0 ); // Enable CTC interrupt
	TCCR0|=(1<<CS02) | (1<<CS00); //Pre-scaler 1024, (42.353 interrupts/s in Normal Mode)
	
	OCR0=0x96; //72 clicks per second
}

//Timer ISR on compare match to run PID
ISR(TIMER0_COMP_vect)
{
	runPIDcount+=1;
	
	if (runPIDcount>=10){
		runPIDcount=0;
		//used for Quadrature Encoder - we will instead use MOUSE to get decoded quadrature code.
		//reading encoders on left and right motor
		scanMouse();
		actualPointLeft=mouseNewY;
		actualPointRight=mouseNewX;
		
		//if(pidSwitchLeft==1) doPIDLeft();

		//if(pidSwitchRight==1) doPIDRight();
		
		if(pidSwitchRight==1 || pidSwitchLeft==1) doPID();
		secondsCount+=1;

		SEND_ON_UART=1;
		/*if (UPDATE_UART==1) {
			itoa( secondsCount, buffer, 10);		// convert integer into string (decimal format)
			strcpy(strbuffer,buffer);

			strcat(strbuffer,",");
			itoa( actualPointLeft*4, buffer, 10);		// convert integer into string (decimal format)
			strcat(strbuffer,buffer);

			strcat(strbuffer,",");
			itoa( actualPointRight*4, buffer, 10);	// convert integer into string (decimal format)
			strcat(strbuffer,buffer);

			strcat(strbuffer,"\r\n");

			uart_puts(strbuffer);					// and transmit string to UART
			
			UPDATE_UART=0;
		}
		*/
	}
}

void doPID(void)
{
	int errorLeft; //error from left encoder
	int errorRight; //error from right encoder
	int dErrorLeft; //derivative error left
	int dErrorRight; //derivative error right
	int cLeft;
	int cRight;

	encoderCountLeft=actualPointLeft;//mouseNewY;
	encoderCountRight=actualPointRight;//mouseNewX;

	errorLeft = setpoint - encoderCountLeft; //calculate error values
	errorRight = setpoint - encoderCountRight;

	//encoderCountLeft = 0; //reset encoder counts ready for next sample
	//encoderCountRight = 0;

	dErrorLeft = (errorLeft - prevErrorLeft);
	dErrorRight = (errorRight - prevErrorRight);

	cLeft = ((KPl*errorLeft) + (KDl*dErrorLeft) + (KIl*IerrorLeft)); //PID equations
	cRight = ((KPr*errorRight) + (KDr*dErrorRight) + (KIr*IerrorRight));

	if(pidSwitchLeft==1) {
		if((OCR1A + cLeft) > 10000) //prevent OCR0 and OCR2 from overrunning 255
		OCR1A = 10000;
		else
		OCR1A = OCR1A + cLeft; //use output from PID equations to alter motor speeds
	}
	if(pidSwitchRight==1) {
		if((OCR1B + cRight) > 10000) //where OCR0 and OCR2 are PWM output values
		OCR1B = 10000;
		else
		OCR1B = OCR1B + cRight;
	}
	
	prevErrorLeft = errorLeft; //set previous error to current error
	prevErrorRight = errorRight;

	IerrorLeft = IerrorLeft + errorLeft; //add current error to integral error
	IerrorRight = IerrorRight + errorRight;
}
/*
void doPIDRight(void)
{
	int errorRight; //error from right encoder
	int dErrorRight; //derivative error right
	int cRight;

	int KP = 10; //PID proportional gain constant
	float KI = 0; //PID integral gain constant
	float KD = 20; //PID derivative gain constant

	encoderCountRight=actualPointRight;

	errorRight = setpoint - encoderCountRight; //calculate error values

	//encoderCountRight = 0; //reset encoder counts ready for next sample

	dErrorRight = (errorRight - prevErrorRight);

	cRight = ((KP*errorRight) + (KD*dErrorRight) + (KI*IerrorRight)); //PID equations

	if((OCR1B + cRight) > 10000) //prevent OCR1B from overrunning 10000, where OCR1B is PWM output values for right motor
		OCR1B = 10000;
	else
		OCR1B = OCR1B + cRight; //use output from PID equations to alter motor speeds

	prevErrorRight = errorRight; //set previous error to current error

	IerrorRight = IerrorRight + errorRight; //add current error to integral error
}

void doPIDLeft(void)
{
	int errorLeft; //error from Left encoder
	int dErrorLeft; //derivative error Left
	int cLeft;

	int KP = 10; //PID proportional gain constant
	float KI = 0; //PID integral gain constant
	float KD = 20; //PID derivative gain constant

	encoderCountLeft=actualPointLeft;

	errorLeft = setpoint - encoderCountLeft; //calculate error values

	//encoderCountLeft = 0; //reset encoder counts ready for next sample

	dErrorLeft = (errorLeft - prevErrorLeft);

	cLeft = ((KP*errorLeft) + (KD*dErrorLeft) + (KI*IerrorLeft)); //PID equations

	if((OCR1A + cLeft) > 10000) //prevent OCR1B from overrunning 10000, where OCR1B is PWM output values for Left motor
	OCR1A = 10000;
	else
	OCR1A = OCR1A + cLeft; //use output from PID equations to alter motor speeds

	prevErrorLeft = errorLeft; //set previous error to current error

	IerrorLeft = IerrorLeft + errorLeft; //add current error to integral error
}
*/